void printdata(void)//degrees(x)
{   
#if PRINT_BINARY != 1  //Print either Ascii or binary messages

  SerialPort.print("!!! ");

#if PRINT_GCS =  = 1
  //ADC
  SerialPort.print("A ");
  SerialPort.print(read_adc(0),0);
  SerialPort.print(" ");
  SerialPort.print(read_adc(1),0);
  SerialPort.print(" ");
  SerialPort.print(read_adc(2),0);
  SerialPort.print(" ");
  SerialPort.print(read_adc(3),0);
  SerialPort.print(" ");
  SerialPort.print(read_adc(4),0);
  SerialPort.print(" ");
  SerialPort.print(read_adc(5),0);
  SerialPort.print(" ");

  //EULER
  SerialPort.print("E ");
  SerialPort.print(degrees(roll),1);
  SerialPort.print(" ");
  SerialPort.print(degrees(pitch),1);
  SerialPort.print(" ");
  SerialPort.print(degrees(yaw),1);
  SerialPort.print(" ");

  //p q r  //corrected
  SerialPort.print("O ");
  SerialPort.print(degrees(Omega_Vector[0]),0);
  SerialPort.print(" ");
  SerialPort.print(degrees(Omega_Vector[1]),0);
  SerialPort.print(" ");
  SerialPort.print(degrees(Omega_Vector[2]),0);
  SerialPort.print(" ");

  //PID
  //roll
  SerialPort.print("R ");
  SerialPort.print(PIDroll.Pterm,1);
  SerialPort.print(" ");
  SerialPort.print(PIDroll.Iterm,1);
  SerialPort.print(" ");
  SerialPort.print(PIDroll.Dterm,1);
  SerialPort.print(" ");
  SerialPort.print(control_roll);
  SerialPort.print(" ");
  SerialPort.print(command_roll.current,1);
  SerialPort.print(" ");
  //pitch
  SerialPort.print("P ");
  SerialPort.print(PIDpitch.Pterm,1);
  SerialPort.print(" ");
  SerialPort.print(PIDpitch.Iterm,1);
  SerialPort.print(" ");
  SerialPort.print(PIDpitch.Dterm,1);
  SerialPort.print(" ");
  SerialPort.print(control_pitch);
  SerialPort.print(" ");
  SerialPort.print(command_pitch.current,1);
  SerialPort.print(" ");
  //yaw
  SerialPort.print("Y ");
  SerialPort.print(PIDyaw.Pterm,1);
  SerialPort.print(" ");
  SerialPort.print(PIDyaw.Iterm,1);
  SerialPort.print(" ");
  SerialPort.print(PIDyaw.Dterm,1);
  SerialPort.print(" ");
  SerialPort.print(control_yaw);
  SerialPort.print(" ");
  SerialPort.print(command_yaw.current,1);
  SerialPort.print(" ");
  //throttle
  SerialPort.print("T ");
  SerialPort.print(control_throttle);
  SerialPort.print(" ");

  //ap mode
  SerialPort.print("M ");
  SerialPort.print(AP_mode+'0'); 
  SerialPort.print(" ");
  SerialPort.print(ServoDecode.getState()+'0'); 
  SerialPort.print(" ");
#endif
  SerialPort.println("***");    

#else
  //  This section outputs a binary data message
  // 68 bytes 11.8ms
  // at 20Hz 236.1ms (13600bps)
  byte IMU_buffer[70];
  int temp_int;

  // start
  IMU_buffer[0] = 0xFA; // 250
  IMU_buffer[1] = 0x02; // 2     

  // Euler angles (°) (x100)
  temp_int = degrees(roll)*100 + 32768;  //Roll (degrees) * 100 in 2 bytes
  IMU_buffer[2] = temp_int&0xff;
  IMU_buffer[3] = (temp_int>>8)&0xff;

  temp_int = degrees(pitch)*100 + 32768;   //Pitch (degrees) * 100 in 2 bytes
  IMU_buffer[4] = temp_int&0xff;
  IMU_buffer[5] = (temp_int>>8)&0xff;

  temp_int = degrees(yaw)*100 + 32768;  //Yaw (degrees) * 100 in 2 bytes
  IMU_buffer[6] = temp_int&0xff;
  IMU_buffer[7] = (temp_int>>8)&0xff;

  // ADC raw data
  temp_int = read_adc(0) + 32768;
  IMU_buffer[8] = temp_int&0xff;
  IMU_buffer[9] = (temp_int>>8)&0xff;

  temp_int = read_adc(1) + 32768; 
  IMU_buffer[10] = temp_int&0xff;
  IMU_buffer[11] = (temp_int>>8)&0xff;

  temp_int = read_adc(2) + 32768;  
  IMU_buffer[12] = temp_int&0xff;
  IMU_buffer[13] = (temp_int>>8)&0xff;

  temp_int = read_adc(3) + 32768;  
  IMU_buffer[14] = temp_int&0xff;
  IMU_buffer[15] = (temp_int>>8)&0xff;

  temp_int = read_adc(4) + 32768; 
  IMU_buffer[16] = temp_int&0xff;
  IMU_buffer[17] = (temp_int>>8)&0xff;

  temp_int = read_adc(5) + 32768; 
  IMU_buffer[18] = temp_int&0xff;
  IMU_buffer[19] = (temp_int>>8)&0xff;
  
  // Omega (PI corrected) (°/s)
  temp_int = degrees(Omega_Vector[0]) + 32768; 
  IMU_buffer[20] = temp_int&0xff;
  IMU_buffer[21] = (temp_int>>8)&0xff;
  
  temp_int = degrees(Omega_Vector[1]) + 32768; 
  IMU_buffer[22] = temp_int&0xff;
  IMU_buffer[23] = (temp_int>>8)&0xff;
  
  temp_int = degrees(Omega_Vector[2]) + 32768; 
  IMU_buffer[24] = temp_int&0xff;
  IMU_buffer[25] = (temp_int>>8)&0xff;
  
  // Radio TX (us)
  temp_int = ch[1] + 32768; 
  IMU_buffer[26] = temp_int&0xff;
  IMU_buffer[27] = (temp_int>>8)&0xff;
  
  temp_int = ch[2] + 32768; 
  IMU_buffer[28] = temp_int&0xff;
  IMU_buffer[29] = (temp_int>>8)&0xff;
  
  temp_int = ch[3] + 32768; 
  IMU_buffer[30] = temp_int&0xff;
  IMU_buffer[31] = (temp_int>>8)&0xff;
  
  temp_int = ch[4] + 32768; 
  IMU_buffer[32] = temp_int&0xff;
  IMU_buffer[33] = (temp_int>>8)&0xff;

  temp_int = ch[5] + 32768; 
  IMU_buffer[34] = temp_int&0xff;
  IMU_buffer[35] = (temp_int>>8)&0xff;
  
  temp_int = ch[6] + 32768; 
  IMU_buffer[36] = temp_int&0xff;
  IMU_buffer[37] = (temp_int>>8)&0xff;  
  
  temp_int = ch[7] + 32768; 
  IMU_buffer[38] = temp_int&0xff;
  IMU_buffer[39] = (temp_int>>8)&0xff;  
  
  temp_int = ch[8] + 32768; 
  IMU_buffer[40] = temp_int&0xff;
  IMU_buffer[41] = (temp_int>>8)&0xff;  
  
  // Control (command laws result) (us)
  temp_int = control_roll + 32768; 
  IMU_buffer[42] = temp_int&0xff;
  IMU_buffer[43] = (temp_int>>8)&0xff; 
  
  temp_int = control_pitch + 32768; 
  IMU_buffer[44] = temp_int&0xff;
  IMU_buffer[45] = (temp_int>>8)&0xff;  
  
  temp_int = control_yaw + 32768; 
  IMU_buffer[46] = temp_int&0xff;
  IMU_buffer[47] = (temp_int>>8)&0xff;  
  
  temp_int = control_throttle + 32768; 
  IMU_buffer[48] = temp_int&0xff;
  IMU_buffer[49] = (temp_int>>8)&0xff;  
  
  // Consignes (°) (x100)
  temp_int = (command_roll.current*100) + 32768; 
  IMU_buffer[50] = temp_int&0xff;
  IMU_buffer[51] = (temp_int>>8)&0xff;  
  
  temp_int = (command_pitch.current*100) + 32768; 
  IMU_buffer[52] = temp_int&0xff;
  IMU_buffer[53] = (temp_int>>8)&0xff;  
  
  temp_int = (command_yaw.current*100) + 32768; 
  IMU_buffer[54] = temp_int&0xff;
  IMU_buffer[55] = (temp_int>>8)&0xff;  
  
  // Omega P correction (°/s)
  temp_int = degrees(Omega_P[0]) + 32768; 
  IMU_buffer[56] = temp_int&0xff;
  IMU_buffer[57] = (temp_int>>8)&0xff;
  
  temp_int = degrees(Omega_P[1]) + 32768; 
  IMU_buffer[58] = temp_int&0xff;
  IMU_buffer[59] = (temp_int>>8)&0xff;
  
  temp_int = degrees(Omega_P[2]) + 32768; 
  IMU_buffer[60] = temp_int&0xff;
  IMU_buffer[61] = (temp_int>>8)&0xff;
  
  // Omega I correction (°/s)
  temp_int = degrees(Omega_I[0]) + 32768; 
  IMU_buffer[62] = temp_int&0xff;
  IMU_buffer[63] = (temp_int>>8)&0xff;
  
  temp_int = degrees(Omega_I[1]) + 32768; 
  IMU_buffer[64] = temp_int&0xff;
  IMU_buffer[65] = (temp_int>>8)&0xff;
  
  temp_int = degrees(Omega_I[2]) + 32768; 
  IMU_buffer[66] = temp_int&0xff;
  IMU_buffer[67] = (temp_int>>8)&0xff;
  
  // AHRS real loop time (ms)
  temp_int = (int)(G_Dt*1000.); 
  IMU_buffer[68] = temp_int&0xff;
  
  // AHRS max real loop time (ms)
  temp_int = (int)(G_Dt_max*1000.); 
  IMU_buffer[69] = temp_int&0xff;
  
  G_Dt_max = 0;
  
  for (int i=0;i<70;i++) 
    SerialPort.print(IMU_buffer[i]);  

#endif  
}


